/**
    Copyright 2015 Table Rider
    By: Anders Nylander
*/

#include "Dope.h"
#include "include/ComClient.h"

Dope::Dope(CordSystem * cord)
{
    this->cord = cord;
    ip = 0; // IP Always set to zero when there is no John to follow
    port = 0;
    JohnX = 0;
    JohnY = 0;
    Doe = false;
    //ctor
}

bool Dope::isDoe(){ return Doe; }

/** @brief setDoe
  *
  * @todo: Assigns a given IP adress and port that points to "John".
  */
void Dope::setDoe(unsigned long ip, unsigned short port)
{
    this->ip = ip;
    this->port = port;
    if (ip != 0 && ip != 0xFFFFFFFF) Doe = true;
    else Doe = false;
}

/** @brief update
  *
  * @todo: document this function
  */
void Dope::update()
{
    if (ip==0) return;
    printf("updating doe\n");
    findJohn();
    if (cord->getStepAmount() == 0){
        makeSteps();
        printf("Making more steps...\n");
    }
    doAllSteps();
}

/** @brief findJohn
  *
  * @todo: document this function
  */
void Dope::findJohn()
{
    const char* str;
    ComClient client;
    client.init(ip, port);
    unsigned short cmd = TBLRDR_GETPOS;
    client.send_msg((void*)&cmd, 2);
    str = client.receive_msg();
    if (str == NULL || str[0] == 'C')
    {
        return;
    }   /// Incase we fail to receive any coordinates
    printf("%s\n", str);
    /// Expected string format: "x:%f\ny:%f\n"
    const char* stringX = (str+2); // x starts at position [2]
    int i=0;
    while(stringX[i] != '\n' && stringX[i] != '\0') i++;
    char* arg = (char*)calloc((i+1),sizeof(char));
    memcpy(arg, stringX, i);
    JohnX = atof(arg);
    free(arg);
    int m=0;
    const char* stringY = (str+2+i+3); // y starts at position [2+i+2];
    while(stringY[m] != '\n' && stringY[m] != '\0') m++;
    arg = (char*)calloc((m+1),sizeof(char));
    for(int k=0;k<m;k++)arg[k] = '\0';
    memcpy(arg, stringY, m);
    JohnY = atof(arg);
    free(arg);
    client.close_connection();
    printf("John Pos\nx:%f\ny:%f\n", JohnX, JohnY);
}

/** @brief makeSteps
  *
  * @todo: document this function
  */
void Dope::makeSteps()
{
    cord->goToJohnPos(JohnX, JohnY);
}


/** @brief doAllSteps
  *
  * @todo: document this function
  */
void Dope::doAllSteps()
{
    while(cord->getStepAmount() > 0){
        cord->doNextStep();
    }
}

bool Dope::isDone(){
    return (cord->getStepAmount() > 0 ? false : true);
}
